#pragma once

#ifndef CAMERA_H
#define CAMERA_H

#include "common_include.h"

namespace test_visual_odom{

class Camera{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    typedef shared_ptr<Camera> Ptr;

    Camera(){};
    Camera(double fx, double fy, double cx, double cy,
           const SE3 &pose)
        : fx_(fx), fy_(fy), cx_(cx), cy_(cy), pose_(pose) {
        pose_inv_ = pose_.inverse();
    }

    SE3 pose() const {
        return pose_;
    }

    //如果没有输入深度，求得的是相机坐标系下，归一化平面的特征点的坐标
    Eigen::Vector3d pixel2camera(const Eigen::Vector2d& pos_pixel, double depth = 1);
    Eigen::Vector2d world2pixel(const Eigen::Vector3d &p_w, const SE3 &T_c_w);
    Eigen::Vector3d world2camera(const Eigen::Vector3d &p_w, const SE3 &T_c_w);
    Eigen::Vector2d camera2pixel(const Eigen::Vector3d &p_c);

    Eigen::Vector4d getCameraIntrinsics(){
        return Eigen::Vector4d(fx_,fy_,cx_,cy_);
    };

private:
    double fx_ = 0, fy_ = 0, cx_ = 0, cy_ = 0;
    SE3 pose_;
    SE3 pose_inv_;

    
};

}

#endif
